
/*
 * Description:
 *		Implementation of ResampleWav and ResampleBuf
 * Author:
 *		Xiao Wu
 * Date:  
 *		2005/04/26
 * Modified:
 *		2005/05/03
 *		2005/05/18
 *		2005/05/19
*/
#include "resample.h"
#include "filter.h"

#include <math.h>
#include <stdio.h>
#include <string.h>

//enable ipp
//#define USE_IPPS
//#define USE_PRINT

//use Intel(R) Performance Primitives(IPP) "Dot Product Functions"
//IPP can greatly increase the performance of resample
#ifdef	USE_IPPS
	#include "ipps.h"		
	#pragma comment(lib, "ippsa6l.lib")
#endif

#ifdef	USE_PRINT
	#define PRINT	printf
#else
	#define PRINT	
#endif

//need CopyFile() API
#ifdef _WIN32
	#include "windows.h" 
#endif 

#define MAX(a,b)	\
	((a) >= (b) ? (a) : (b))

#define MIN(a,b)	\
	((a) <= (b) ? (a) : (b))

#define ABS(a)		\
	((a) >=0 ? (a) : (-a))

//round 'a' up with factor 'b'. ie: Round_up(43,8) = 48
#define ROUND_UP(a,b)	\
	(int(ceil(float(a)/float(b))) * int(b))

//round 'a' down with factor 'b'. ie: Round_up(43,8) = 40
#define ROUND_DOWN(a,b)	\
	(int(floor(float(a)/float(b))) * int(b))


#define BUFFER_CHUNK			(65536U)	//buffer chunk
#define THRESHOLD_WINDOW_SZ		(8192U)		//threshold of the low pass filter

//wave header position
#define WAVE_TAGRIFF	0U
#define WAVE_CHUNK		4U
#define WAVE_TAGWAVE	8U
#define WAVE_TAGFMT		12U
#define WAVE_CHUNKSUB	16U
#define WAVE_FMT		20U
#define WAVE_CHANNEL	22U
#define WAVE_SAMPLERATE	24U
#define WAVE_BYTERATE	28U
#define WAVE_BLOCK		32U
#define WAVE_BITENCODE	34U
#define WAVE_TAGDATA	36U
#define WAVE_CHUNKDATA	40U

struct WavFormat
{
	size_t szChunk;
	size_t szChunkSub;		//must be 16 for PCM wav
	size_t szChunkData;		//must be szChunk-36 for PCM wav
	size_t sampleRate;
	unsigned short numChannel;
	unsigned short audioFormat;
	unsigned short bitEncode;
};

extern int _Buf2WF(char* header, WavFormat& format);	
extern int _WF2Buf(char* header, WavFormat& format);	
extern void _SimplestNum(int& num1, int& num2);			
extern size_t _buf2filter(const float* src, short* dst, size_t srclen, int& SubLp, float* coeffp, int subN, int Ufactor, int Dfactor);
extern size_t _ResampleWav(const char* WavIn, const char* WavOut, int Ufactor, int Dfactor, int ChannelMode = CHANNEL_LEFT);
extern size_t _ResampleBuf(const short* src, short* dst, size_t srclen, int Ufactor, int Dfactor, int srcChannel);


size_t ResampleWav(const char* WavIn, const char* WavOut, int OutSampleRate, int ChannelMode)
{	
	PRINT("----------------------------------------------------------------------------\n");
	PRINT("ResampleWav to %uHz\n",OutSampleRate);
	PRINT("Source Wav: %s\n",WavIn);
	PRINT("Target Wav: %s\n",WavOut);
	FILE* pfIn  = fopen(WavIn,  "rb");
	if (!pfIn)
	{
		fprintf(stderr,"[ERROR!]: Cannot open \'%s\'.\n",WavIn);
		return 0;
	}
	char wavHead[44] = {0};
	WavFormat WFIn;

	if (44 != fread(wavHead,sizeof(char),44,pfIn))
	{
		fprintf(stderr,"[ERROR!]: Cannot read wave header.\n");
		return 0;
	}
	
	if (!_Buf2WF(wavHead,WFIn))
	{
		fprintf(stderr,"[ERROR!]: invalid wav header. Please check the header tag.\n");
		return 0;
	}
	fclose(pfIn);
	return _ResampleWav(WavIn,WavOut,OutSampleRate,WFIn.sampleRate,ChannelMode);
}
//////////////////////////////////////////////////////////////////////////


size_t ResampleBuf(const short* src, short* dst, size_t srclen, int Ufactor, int Dfactor, int srcChannel)
{
	PRINT("----------------------------------------------------------------------------\n");
	PRINT("ResampleBuf, Ufactor %u, Dfactor %u\n",Ufactor,Dfactor);
	if (!src || !dst || !srclen || Ufactor<=0 || Dfactor<=0)
	{
		fprintf(stderr,"[ERROR!]: Invalid input parameters.\n");
		return 0;
	}
	if (srcChannel != MONO && srcChannel != STEREO)
	{
		fprintf(stderr,"[ERROR!]: Only mono and 2Chan stereo are supported.\n");
		return 0;
	}
	return _ResampleBuf(src,dst,srclen,Ufactor,Dfactor,srcChannel);
}
//////////////////////////////////////////////////////////////////////////


//convert 44 bytes char string to WavFormat struct
int _Buf2WF(char* header, WavFormat& format)
{
	if (strncmp(header,"RIFF",4))
		return 0;
	format.szChunk = *(size_t*)(header+WAVE_CHUNK);
	format.szChunkSub = *(size_t*)(header+WAVE_CHUNKSUB);
	format.szChunkData = *(size_t*)(header+WAVE_CHUNKDATA);
	format.sampleRate = *(size_t*)(header+WAVE_SAMPLERATE);
	format.bitEncode = *(unsigned short*)(header+WAVE_BITENCODE);
	format.numChannel = *(unsigned short*)(header+WAVE_CHANNEL);
	format.audioFormat = *(unsigned short*)(header+WAVE_FMT);
	return 1;
}
//////////////////////////////////////////////////////////////////////////


//convert WavFormat struct to 44 bytes char string
int _WF2Buf(char* header, WavFormat& format)
{
	strncpy(header+WAVE_TAGRIFF,"RIFF",4);
	*(size_t*)(header+WAVE_CHUNK) = format.szChunk;
	strncpy(header+WAVE_TAGWAVE,"WAVE",4);
	strncpy(header+WAVE_TAGFMT,"fmt ",4);
	*(size_t*)(header+WAVE_CHUNKSUB) = format.szChunkSub;
	*(unsigned short*)(header+WAVE_FMT) = format.audioFormat;
	*(unsigned short*)(header+WAVE_CHANNEL) = format.numChannel;
	*(size_t*)(header+WAVE_SAMPLERATE) = format.sampleRate;
	*(unsigned short*)(header+WAVE_BITENCODE) = format.bitEncode;
	*(unsigned short*)(header+WAVE_BLOCK) = format.numChannel*format.bitEncode/8;
	*(size_t*)(header+WAVE_BYTERATE) = format.sampleRate*format.numChannel*format.bitEncode/8;
	strncpy(header+WAVE_TAGDATA,"data",4);
	*(size_t*)(header+WAVE_CHUNKDATA) = format.szChunkData;
	return 1;
}
//////////////////////////////////////////////////////////////////////////


//print out wave information
void _PrintWavMessage(WavFormat& format)
{
	
	int hour=0,min=0,sec=0,msec=0;
	float t = float(format.szChunkData)/
		(float(format.sampleRate)*float(format.bitEncode/8)*float(format.numChannel));
	hour = (int)(t/3600.0F);
	t -= hour*3600;
	min = (int)(t/60.0F);
	t -= min*60;
	sec = (int)t;
	t -= sec;
	msec = (int)(t*1000);
	PRINT("*************************************************************\n");
	PRINT("  Wave Information\n");
	PRINT("  Hour: %-16u Minute: %-6u  Second: %u.%.3u\n",hour,min,sec,msec);
	PRINT("  FileSize: %-12u Channel: %-7uEncoding: %ubits\n",format.szChunk+8,format.numChannel,format.bitEncode);
	PRINT("  DataSize: %-12u FormatID: %-6uSampleRate: %u\n",format.szChunkData,format.audioFormat,format.sampleRate);
	PRINT("*************************************************************\n");
	
}
//////////////////////////////////////////////////////////////////////////


//Compute the submultiple of num1 and num2, and make them simplest number.
//ie. Input:	num1=44100  num2=8000
//	  Output:	num1=441	num2=80
void   _SimplestNum(int& num1, int& num2)
{
	if (!num1 || !num2)
		return;
	if (num1 == num2)
	{
		num1 = num2 = 1;
		return;
	}
	int& max = num1 > num2 ? num1 : num2;
	int& min = num1 < num2 ? num1 : num2;
	int threshold = min/2 + 1;
	for (int divisor=2; divisor<threshold; ++divisor)
	{
		while(!(num1%divisor) && !(num2%divisor))
		{
			num1 /= divisor;
			num2 /= divisor;
		}
	}

}
//////////////////////////////////////////////////////////////////////////


//put buffer into low-pass filter
//the filter is polyphase decomposed into Ufactor sub-filter
//SubLp is the current index of sub-filter 
size_t _buf2filter(const float* src, short* dst, size_t srclen, int& SubLp, float* coeffp, int subN, int Ufactor, int Dfactor)
{
	const short* oriDst = dst;
	const float* oriSrc = src;
	const float* lastSrc = src + srclen - subN;
	 
	float sum = 0;
	for (; SubLp>=Ufactor && src<lastSrc; SubLp-=Ufactor)
		++src;
	while (src < lastSrc)
	{
		sum = 0;
#ifdef USE_IPPS
		ippsDotProd_32f(coeffp+SubLp*subN, src, subN, &sum);
#else
		float* pcoe = coeffp+SubLp*subN;
		for (int i=0; i<subN; ++i)
			sum += pcoe[i] * src[i];	
#endif
		if (sum > 32767.0)  
			sum = 32767.0;
		else if (sum < -32767.0)
			sum = -32767.0;
		
		*dst = (short)sum;
		++dst;
		for (SubLp += Dfactor; SubLp>=Ufactor && src<lastSrc; SubLp-=Ufactor)
			++src;
	}
	return dst-oriDst;

}
//////////////////////////////////////////////////////////////////////////
#ifndef _WIN32
bool CopyFile(const char *infile, const char *outfile, bool bText)
{
	FILE *a, *b;

	a = fopen(infile, "rb");
	if (a==NULL) {printf("Can't open %s\n", infile); return false;}

	b = fopen(outfile, "wb+");
	if (b==NULL) {printf("Can't open %s\n", outfile); return false;}

	char *data;
	int  len;
	struct stat mybuf;
	fstat(fileno(a),&mybuf);
	len = mybuf.st_size;
	data = new char [len];
	if (data==NULL) {printf("%s is too big(%d bytes)!\n", infile, len); return false;}
	fread(data, len, sizeof(char), a);
	fclose(a);

	int nW;
	nW = fwrite(data, len, sizeof(char), b);
	fclose(b);
	delete data;
	if (nW!=len) {printf("write %s failed(read num != write num)\n", outfile); return false;}
	return true;	
}
#endif

size_t _ResampleWav(const char* WavIn, const char* WavOut, int Ufactor, int Dfactor, int ChannelMode)
{
	if (!WavIn || !WavOut || Ufactor<=0 || Dfactor<=0)
	{
		fprintf(stderr,"[ERROR!]: Invalid parameters!\n");
		return 0;
	}
	FILE* pfIn  = fopen(WavIn, "rb"); 
	if (!pfIn)
	{
		fprintf(stderr,"[ERROR!]: Cannot open \'%s\'.\n",WavIn);
		return 0;
	}
	FILE* pfOut = fopen(WavOut, "w+b");
	if (!pfOut)
	{
		fprintf(stderr,"[ERROR!]: Cannot open \'%s\'.\n",WavOut);
		return 0;
	}
	
	//read wav header and check valid or not
	char wavHead[44] = {0};
	WavFormat WFIn, WFOut;
	if (44 != fread(wavHead,sizeof(char),44,pfIn))
	{
		fprintf(stderr,"[ERROR!]: cannot read wav header.\n");
		return 0;
	}
	if (!_Buf2WF(wavHead,WFIn))
	{
		fprintf(stderr,"[ERROR!]: invalid wav header. Please check the header tag.\n");
		return 0;
	}

	//print wave header information
	PRINT("Source wav information:\n");
	_PrintWavMessage(WFIn);
	if (WFIn.bitEncode != 16 || WFIn.audioFormat != 1 || (WFIn.numChannel != MONO && WFIn.numChannel != STEREO))
	{
		fprintf(stderr,"[ERROR!]: Unsupported wave. Please check the wave are (mono|stereo)(16bit)(PCM).\n");
		return 0;
	}
	
	//simply Copyfile while the resample factor = 1
	if (Ufactor == Dfactor && WFIn.numChannel == MONO)
	{
		fprintf(stderr,"[WARNNING!]: Do filecopy while source SampleRate equals to target SampleRate.\n");
		fclose(pfIn);
		fclose(pfOut);
		if (!CopyFile(WavIn, WavOut, false))
			fprintf(stderr, "[ERROR!]: Unknown error occurs while copying file.\n");
		return 1;
	}

	//write the wave header
	fwrite(wavHead,sizeof(char),44,pfOut); 
	
	//compute [Ufactor,Dfactor]   
	int oriUfactor = Ufactor;
	int oriDfactor = Dfactor;
	_SimplestNum(Ufactor, Dfactor);

	//compute the lowpass filter coefficient
	//polyphase decomposition: coeff -> coeffp
	//the filter are decomposed to Ufactor sub-filters
	float cutoff = 0.5F/(float)MAX(Ufactor,Dfactor);
	int N = MIN(int(float(FILTER_ORDER)*0.5/cutoff),THRESHOLD_WINDOW_SZ);
	int SubN = (int)ceil(float(2*N-1)/Ufactor);
	float* coeff  = new float[N];
	float* coeffp = new float[SubN*Ufactor];
	LpFilter(coeff,cutoff,N,(float)Ufactor);
	PolyPhaseDecomposition(coeff, coeffp, N, Ufactor);
	
	//BufIn: WavIn buffer
	//BufTmp: BufIn short -> BufTmp float
	//BufOut: WavOut Buffer
	const size_t szBufIn  = BUFFER_CHUNK;
	const size_t szBufTmp = BUFFER_CHUNK/WFIn.numChannel;
	const size_t szBufOut = szBufIn*Ufactor/Dfactor+1; 
	
	short* BufIn  = new short[szBufIn]; 
	short* BufOut = new short[szBufOut];
	float* BufTmp = new float[szBufTmp];
		
	//szRd: size of read
	//szWt: size of write
	//szDataOut: sum of szWt
	//prefill: overlapped samples before readin samples 
	//startSubLp: index of the startup sub-filter
	size_t szRd = 0;
	size_t szWt = 0;
	size_t szDataOut = 0;
	int prefill = (int)ceil(float(N-Ufactor)/float(Ufactor));
	int startSubLp = prefill*Ufactor - (N-Ufactor);	
	
	memset(BufTmp, 0, szBufTmp*sizeof(float));
	szRd = fread(BufIn, sizeof(short), szBufIn-(prefill<<(WFIn.numChannel-1)), pfIn);
	if (WFIn.numChannel == STEREO)
		szRd >>= 1;

	float* pTmp = 0;
	const short* pInF = 0;
	const short* pInL = 0;
	//enter "while" if szRd+prefill>SubN for the first time, enter "while" if szRd>0 
	//after the first time. the while loop must maintain the overlapped samples and
	//current index of sub-filter.
	while ((int)(szRd+prefill) > SubN)
	{	
		if (WFIn.numChannel == MONO)
		{
			for (int i=0,j=prefill; i<(int)szRd; ++i,++j)
				BufTmp[j] = BufIn[i];
		}
		else if (WFIn.numChannel == STEREO)
		{
			switch(ChannelMode)
			{
			case CHANNEL_MEAN:		
				for(pTmp=BufTmp+prefill,pInF=BufIn,pInL=BufIn+(szRd<<1); pInF<pInL; ++pTmp, pInF+=2)
					*pTmp = (float)((int(*pInF) + int(*(pInF+1)))>>1);
				break;
			case CHANNEL_RIGHT:
				for(pTmp=BufTmp+prefill,pInF=BufIn+1,pInL=BufIn+(szRd<<1); pInF<pInL; ++pTmp, pInF+=2)
					*pTmp = *pInF;
				break;
			case CHANNEL_LEFT:
			default:
				for(pTmp=BufTmp+prefill,pInF=BufIn,pInL=BufIn+(szRd<<1); pInF<pInL; ++pTmp, pInF+=2)
					*pTmp = *pInF;
				break;
			}
		}

		szWt = _buf2filter(BufTmp,BufOut,szRd+prefill,startSubLp,coeffp,SubN,Ufactor,Dfactor);
		fwrite(BufOut, sizeof(short), szWt, pfOut);
		szDataOut += szWt;

		memmove(BufTmp, BufTmp+szRd+prefill-SubN, SubN*sizeof(float));
		szRd = fread(BufIn, sizeof(short),szBufIn-(SubN<<(WFIn.numChannel-1)), pfIn);
		if (WFIn.numChannel == STEREO)
			szRd >>= 1;
		prefill = SubN;	
	}
	
	//"prefill==subN" indicates "while" condition has been met at least once.
	if (prefill != SubN)
		szWt = szRd? (unsigned int)ceil(float(szRd*Ufactor)/float(Dfactor)): 0;
	else 
		szWt = (unsigned int)ceil(float(SubN*Ufactor-N+Ufactor-startSubLp)/float(Dfactor));
	memset(BufTmp+szRd+prefill, 0, SubN*sizeof(float));
	if (WFIn.numChannel == MONO)
	{
		for (int i=0,j=szRd; i<j; ++i)
			BufTmp[i+prefill] = BufIn[i];
	}
	else if (WFIn.numChannel == STEREO)
	{
		int i=0,j=0;
		switch(ChannelMode)
		{
		case CHANNEL_MEAN:
			for (i=0,j=szRd; i<j; ++i)
				BufTmp[i+prefill] = (float)((int(BufIn[i<<1])+int(BufIn[i<<1+1]))>>1);
			break;
		case CHANNEL_RIGHT:
			for (i=0,j=szRd; i<j; ++i)
				BufTmp[i+prefill] = BufIn[i<<1+1];
			break;
		case CHANNEL_LEFT:
		default:
			for (i=0,j=szRd; i<j; ++i)
				BufTmp[i+prefill] = BufIn[i<<1];
			break;
		}
	}
	_buf2filter(BufTmp,BufOut,szRd+prefill+SubN,startSubLp,coeffp,SubN,Ufactor,Dfactor);
	
	fwrite(BufOut, sizeof(short), szWt, pfOut);
	szDataOut += szWt;
	
	//fill in wave header
	WFOut.bitEncode = 16;
	WFOut.audioFormat = 1;
	WFOut.numChannel = 1;
	WFOut.sampleRate = WFIn.sampleRate/oriDfactor*oriUfactor;
	WFOut.szChunkData = szDataOut*sizeof(short);
	WFOut.szChunk = szDataOut*sizeof(short) + 36;
	WFOut.szChunkSub = 16;
	
	//rewrite the header of output file
	_WF2Buf(wavHead,WFOut);
	PRINT("Target wav information:\n");
	_PrintWavMessage(WFOut);
	fseek(pfOut, 0, SEEK_SET);
	fwrite(wavHead,sizeof(char),44,pfOut);
	
	fclose (pfIn);
	fclose (pfOut);	
	delete[] coeff;
	delete[] coeffp;
	delete[] BufIn;
	delete[] BufOut;
	delete[] BufTmp;
	return 1;
}
//////////////////////////////////////////////////////////////////////////


size_t _ResampleBuf(const short* src, short* dst, size_t srclen, int Ufactor, int Dfactor, int srcChannel)
{
	if (!src || !dst || srclen<=0 || Ufactor<=0 || Dfactor<=0)
	{
		fprintf(stderr,"[ERROR!]: Invalid parameters!\n");
		return 0;
	}

	//simply memcpy while the resample factor = 1
	if (Ufactor == Dfactor && srcChannel == MONO)
	{
		fprintf(stderr,"[WARNNING!]: Do memcopy while resample factor equals 1.\n");
		memcpy(dst, src, sizeof(short)*srclen);
		return 1;
	}

	const short* oriDst = dst;
	//compute [Ufactor,Dfactor]   
	int oriUfactor = Ufactor;
	int oriDfactor = Dfactor;
	_SimplestNum(Ufactor, Dfactor);

	//compute the lowpass filter coefficient
	//polyphase decomposition: coeff -> coeffp
	//the filter are decomposed to Ufactor sub-filters
	float cutoff = 0.5F/(float)MAX(Ufactor,Dfactor);
	int N = MIN(int(float(FILTER_ORDER)*0.5/cutoff),THRESHOLD_WINDOW_SZ);
	int SubN = (int)ceil(float(2*N-1)/Ufactor);
	float* coeff  = new float[N];
	float* coeffp = new float[SubN*Ufactor];
	LpFilter(coeff,cutoff,N,(float)Ufactor);
	PolyPhaseDecomposition(coeff, coeffp, N, Ufactor);
	
	//BufTmp: src short -> BufTmp float
	const size_t szBufTmp  = BUFFER_CHUNK;
	float* BufTmp = new float[szBufTmp];
	
	//szRd: size of read
	//szWt: size of write
	//szDataOut: sum of szWt
	//prefill: overlapped samples before readin samples 
	//startSubLp: index of the startup sub-filter
	size_t szRd = 0;
	size_t szWt = 0;
	size_t szLeft = srclen;

	int prefill = (int)ceil(float(N-Ufactor)/float(Ufactor));
	int startSubLp = prefill*Ufactor - (N-Ufactor);
	memset(BufTmp, 0, szBufTmp*sizeof(float));
	
	//szRd = fread(BufIn+prefill, sizeof(short),szBufIn-prefill, pfIn);
	szRd = MIN(szLeft, (szBufTmp-prefill)<<(srcChannel-1));
	szLeft -= szRd;
	if (srcChannel == STEREO)
		szRd >>= 1;
	
	//enter "while" if szRd+prefill>SubN for the first time, enter "while" if szRd>0 
	//after the first time. the while loop must maintain the overlapped samples and
	//current index of sub-filter.
	float* pTmp = 0;
	const short* pInF = 0;
	const short* pInL = 0;
	while ((int)(szRd+prefill) > SubN)
	{	
		if (srcChannel == MONO)			
		{
			for (int i=0,j=prefill; i<(int)szRd; ++i,++j)
				BufTmp[j] = src[i];
		}
		else if (srcChannel == STEREO)
		{
			for(pTmp=BufTmp+prefill,pInF=src,pInL=src+(szRd<<1); pInF<pInL; ++pTmp, pInF+=2)
				*pTmp = (float)((int(*pInF) + int(*(pInF+1)))>>1);
		}

		szWt = _buf2filter(BufTmp,dst,szRd+prefill,startSubLp,coeffp,SubN,Ufactor,Dfactor);
		src += (szRd<<(srcChannel-1));
		dst += szWt;
		
		memmove(BufTmp, BufTmp+szRd+prefill-SubN, SubN*sizeof(float));
		szRd = MIN(szLeft, (szBufTmp-SubN)<<(srcChannel-1));
		szLeft -= szRd;
		prefill = SubN;
		if (srcChannel == STEREO)
			szRd >>= 1;
	}
		
	//"prefill==subN" indicates "while" condition has been met at least once. 
	if (prefill != SubN)
		szWt = szRd? (unsigned int)ceil(float(szRd*Ufactor)/float(Dfactor)): 0;
	else 
		szWt = (unsigned int)ceil(float(SubN*Ufactor-N+Ufactor-startSubLp)/float(Dfactor));
	memset(BufTmp+szRd+prefill, 0, SubN*sizeof(float));
	memset(dst, 0, szWt*sizeof(short));
	if (srcChannel == MONO)	
		for (int i=0,j=szRd; i<j; ++i)
			BufTmp[i+prefill] = src[i];
	else if (srcChannel == STEREO)
		for (int i=0,j=szRd; i<j; ++i)
			BufTmp[i+prefill] = (float)((int(src[i<<1]) + int(src[i<<1+1]))>>1);
	
	/*Modified by xwu at 2005/5/18
	_buf2filter(BufTmp,BufOut,szRd+prefill+SubN,startSubLp,coeffp,SubN,Ufactor,Dfactor);
	*/
	_buf2filter(BufTmp,dst,MIN(szRd+prefill+SubN,(szWt-1)*Dfactor/Ufactor+SubN),startSubLp,coeffp,SubN,Ufactor,Dfactor);  
	dst += szWt;
	
	delete[] coeff;
	delete[] coeffp;
	delete[] BufTmp;
	return dst-oriDst;
}
//////////////////////////////////////////////////////////////////////////
